{
  "cells": [
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "%matplotlib inline"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "\n********************************************************************************\nIMU-GNSS Sensor-Fusion on the KITTI Dataset\n********************************************************************************\nGoals of this script:\n\n- apply the UKF for estimating the 3D pose, velocity and sensor biases of a\n  vehicle on real data.\n\n- efficiently propagate the filter when one part of the Jacobian is already\n  known. \n\n- efficiently update the system for GNSS position.\n\n*We assume the reader is already familiar with the approach described in the\ntutorial and in the 2D SLAM example.*\n\nThis script proposes an UKF to estimate the 3D attitude, the velocity, and the\nposition of a rigid body in space from inertial sensors and position\nmeasurement.\n\nWe use the KITTI data that can be found in the `iSAM repo\n<https://github.com/borglab/gtsam/blob/develop/matlab/gtsam_examples/IMUKittiExampleGNSS.m>`_\n(examples folder).\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Import\n==============================================================================\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "from scipy.linalg import block_diag\nimport ukfm\nimport numpy as np\nimport matplotlib\nukfm.set_matplotlib_config()"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Model and Data\n==============================================================================\nThis script uses the :meth:`~ukfm.IMUGNSS` model that loads the KITTI data\nfrom text files. The model is the standard 3D kinematics model based on\ninertial inputs.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "MODEL = ukfm.IMUGNSS\n# observation frequency (Hz)\nGNSS_freq = 1\n# load data\nomegas, ys, one_hot_ys, t = MODEL.load(GNSS_freq)\nN = t.shape[0]\n# IMU noise standard deviation (noise is isotropic)\nimu_std = np.array([0.01,     # gyro (rad/s)\n                    0.05,     # accelerometer (m/s^2)\n                    0.000001, # gyro bias (rad/s^2)\n                    0.0001])  # accelerometer bias (m/s^3)\n# GNSS noise standard deviation (m)\nGNSS_std = 0.05"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "The state and the input contain the following variables:\n\n.. highlight:: python\n.. code-block:: python\n\n   states[n].Rot     # 3d orientation (matrix)\n   states[n].v       # 3d velocity\n   states[n].p       # 3d position\n   states[n].b_gyro  # gyro bias\n   states[n].b_acc   # accelerometer bias\n   omegas[n].gyro    # vehicle angular velocities\n   omegas[n].acc     # vehicle specific forces\n\nA measurement ``ys[k]`` contains a GNSS (position) measurement.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Filter Design and Initialization\n------------------------------------------------------------------------------\nWe now design the UKF on parallelizable manifolds. This script embeds the\nstate in $SO(3) \\times \\mathbb{R}^{12}$, such that:\n\n* the retraction $\\varphi(.,.)$ is the $SO(3)$ exponential for\n  orientation, and the vector addition for the remaining part of the\n  state.\n\n* the inverse retraction $\\varphi^{-1}_.(.)$ is the $SO(3)$\n  logarithm for orientation and the vector subtraction for the remaining part\n  of the state.\n\nRemaining parameter setting is standard.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "# propagation noise covariance matrix\nQ = block_diag(imu_std[0]**2*np.eye(3), imu_std[1]**2*np.eye(3),\n               imu_std[2]**2*np.eye(3), imu_std[3]**2*np.eye(3))\n# measurement noise covariance matrix\nR = GNSS_std**2 * np.eye(3)"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "We use the UKF that is able to infer Jacobian to speed up the update step, see\nthe 2D SLAM example.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "# sigma point parameters\nalpha = np.array([1e-3, 1e-3, 1e-3, 1e-3, 1e-3])\n# for propagation we need the all state\nred_idxs = np.arange(15)  # indices corresponding to the full state in P\n# for update we need only the state corresponding to the position\nup_idxs = np.array([6, 7, 8])"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "We initialize the state with zeros biases. The initial covariance is non-null\nas the state is not perfectly known.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "# initial uncertainty matrix\nP0 = block_diag(0.01*np.eye(3), 1*np.eye(3), 1*np.eye(3),\n                0.001*np.eye(3), 0.001*np.eye(3))\n# initial state\nstate0 = MODEL.STATE(\n    Rot=np.eye(3),\n    v=np.zeros(3),\n    p=np.zeros(3),\n    b_gyro=np.zeros(3),\n    b_acc=np.zeros(3))"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "As the noise affecting the dynamic of the biases is trivial (it is the\nidentity), we set our UKF with a reduced propagation noise covariance, and\nmanually set the remaining part of the Jacobian.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "# create the UKF\nukf = ukfm.JUKF(state0=state0, P0=P0, f=MODEL.f, h=MODEL.h, Q=Q[:6, :6],\n                phi=MODEL.phi, alpha=alpha, red_phi=MODEL.phi,\n                red_phi_inv=MODEL.phi_inv, red_idxs=red_idxs,\n                up_phi=MODEL.up_phi, up_idxs=up_idxs)\n# set variables for recording estimates along the full trajectory\nukf_states = [state0]\nukf_Ps = np.zeros((N, 15, 15))\nukf_Ps[0] = P0\n# the part of the Jacobian that is already known.\nG_const = np.zeros((15, 6))\nG_const[9:] = np.eye(6)"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Filtering\n==============================================================================\nThe UKF proceeds as a standard Kalman filter with a for loop.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "# measurement iteration number\nk = 1\nfor n in range(1, N):\n    # propagation\n    dt = t[n]-t[n-1]\n    ukf.state_propagation(omegas[n-1], dt)\n    ukf.F_num(omegas[n-1], dt)\n    # we assert the reduced noise covariance for computing Jacobian.\n    ukf.Q = Q[:6, :6]\n    ukf.G_num(omegas[n-1], dt)\n    # concatenate Jacobian\n    ukf.G = np.hstack((ukf.G, G_const*dt))\n    # we assert the full noise covariance for uncertainty propagation.\n    ukf.Q = Q\n    ukf.cov_propagation()\n    # update only if a measurement is received\n    if one_hot_ys[n] == 1:\n        ukf.update(ys[k], R)\n        k = k + 1\n    # save estimates\n    ukf_states.append(ukf.state)\n    ukf_Ps[n] = ukf.P"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Results\n------------------------------------------------------------------------------\nWe plot the estimated trajectory.\n\n"
      ]
    },
    {
      "metadata": {
        "collapsed": false
      },
      "cell_type": "code",
      "source": [
        "MODEL.plot_results(ukf_states, ys)"
      ],
      "outputs": [],
      "execution_count": null
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Results are coherent with the GNSS. As the GNSS is used in the filter, it\nmakes no sense to compare the filter outputs to the same measurement.\n\n"
      ]
    },
    {
      "metadata": {},
      "cell_type": "markdown",
      "source": [
        "Conclusion\n==============================================================================\nThis script implements an UKF for sensor-fusion of an IMU with GNSS. The UKF\nis efficiently implemented, as some part of the Jacobian are known and not\ncomputed. Results are satisfying.\n\nYou can now:\n\n* increase the difficulties of the example by reduced the GNSS frequency or\n  adding noise to position measurements.\n\n* implement the UKF with different uncertainty representations, as viewing the\n  state as an element $\\boldsymbol{\\chi} \\in SE_2(3) \\times\n  \\mathbb{R}^6$. We yet provide corresponding retractions and inverse\n  retractions.\n\n"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "file_extension": ".py",
      "version": "3.5.2",
      "pygments_lexer": "ipython3",
      "name": "python",
      "codemirror_mode": {
        "version": 3,
        "name": "ipython"
      },
      "mimetype": "text/x-python",
      "nbconvert_exporter": "python"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}